/*
 * @Author: Mcfly
 * @Date: 2021-07-17 11:37:53
 * @LastEditTime: 2021-11-01 19:57:06
 * @LastEditors: Mcfly
 * @DescriplaneAttition: 姿态解算
 * @FileplaneAttiath: \FTTfly\Ctrl\Atti.c
 * 本人github:httplaneAttis://github.com/McflyWZX
 */

#include "Atti.h"

Atti planeAtti;

void QtoRPY(void);
Atti Atti_Update(Vector3f *gyro, Vector3f *accel);

void Atti_Init(void)
{
	planeAtti.w = 1;
	planeAtti.q1 = 0;
	planeAtti.q2 = 0;
	planeAtti.q3 = 0;

	planeAtti.yaw = planeAtti.pitch = planeAtti.roll = 0;
}


Atti *getPlaneAtti(void)
{
	return &planeAtti;
}


//---------------------------------------------------------------------------------------------------
// Definitions

const float sampleFreq = 200.0f;			// sample frequency in Hz

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float twoKp = (2.0f * 0.5f);											// 2 * proportional gain (Kp)
volatile float twoKi = (2.0f * 0.0f);											// 2 * integral gain (Ki)
//volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki

float invSqrt(float a)
{
	return 1.0f/sqrtf(a);
}

/**
 * @description: 更新当前姿态
 * @param {Vector3f} *gyro	当前角速度，弧度
 * @param {Vector3f} *accel
 * @return {Atti} 当前姿态，角度表示
 * @Author: Mcfly
 * @Date: 2021-07-17 21:29:09
 */
Atti Atti_Update(Vector3f *gyro, Vector3f *accel) {
	//float originAz = accel->z;
	Vector3f originG = {gyro->x, gyro->y, gyro->z};
	Vector3f originA = {accel->x, accel->y, accel->z};
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;
	float qa, qb, qc;
	
	planeAtti.yaw += gyro->z / sampleFreq * 180 / 3.1415926535f; 
	if(planeAtti.yaw > 180)
		planeAtti.yaw -= 360;
	else if(planeAtti.yaw < -180)
		planeAtti.yaw += 360;

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    // 只在加速度计数据有效时才进行运算
	if(!((accel->x == 0.0f) && (accel->y == 0.0f) && (accel->z == 0.0f))) {

		// Normalise accelerometer measurement
        // 将加速度计得到的实际重力加速度向量v单位化
		recipNorm = invSqrt(accel->x * accel->x + accel->y * accel->y + accel->z * accel->z); //该函数返回平方根的倒数
		accel->x *= recipNorm;
		accel->y *= recipNorm;
		accel->z *= recipNorm;        

		// Estimated direction of gravity
        // 通过四元数得到理论重力加速度向量g 
        // 注意，这里实际上是矩阵第三列*1/2，在开头对Kp Ki的宏定义均为2*增益
        // 这样处理目的是减少乘法运算量
		halfvx = planeAtti.q1 * planeAtti.q3 - planeAtti.w * planeAtti.q2;
		halfvy = planeAtti.w * planeAtti.q1 + planeAtti.q2 * planeAtti.q3;
		halfvz = planeAtti.w * planeAtti.w - 0.5f + planeAtti.q3 * planeAtti.q3;
	
		// Error is sum of cross product between estimated and measured direction of gravity
        // 对实际重力加速度向量v与理论重力加速度向量g做外积
		halfex = (accel->y * halfvz - accel->z * halfvy);
		halfey = (accel->z * halfvx - accel->x * halfvz);
		halfez = (accel->x * halfvy - accel->y * halfvx);

		// Compute and apply integral feedback if enabled
        // 在PI补偿器中积分项使能情况下计算并应用积分项
		if(twoKi > 0.0f) {
            // integral error scaled by Ki
            // 积分过程
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
            
            // apply integral feedback
            // 应用误差补偿中的积分项
			gyro->x += integralFBx;	
			gyro->y += integralFBy;
			gyro->z += integralFBz;
		}
		else {
            // prevent integral windup
            // 避免为负值的Ki时积分异常饱和
			integralFBx = 0.0f;	
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
        // 应用误差补偿中的比例项
		gyro->x += twoKp * halfex;
		gyro->y += twoKp * halfey;
		gyro->z += twoKp * halfez;
	}
	
	// Integrate rate of change of quaternion
    // 微分方程迭代求解
	gyro->x *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
	gyro->y *= (0.5f * (1.0f / sampleFreq));
	gyro->z *= (0.5f * (1.0f / sampleFreq));
	qa = planeAtti.w;
	qb = planeAtti.q1;
	qc = planeAtti.q2;
	planeAtti.w += (-qb * gyro->x - qc * gyro->y - planeAtti.q3 * gyro->z); 
	planeAtti.q1 += (qa * gyro->x + qc * gyro->z - planeAtti.q3 * gyro->y);
	planeAtti.q2 += (qa * gyro->y - qb * gyro->z + planeAtti.q3 * gyro->x);
	planeAtti.q3 += (qa * gyro->z + qb * gyro->y - qc * gyro->x); 
	
	// Normalise quaternion
    // 单位化四元数 保证四元数在迭代过程中保持单位性质
	recipNorm = invSqrt(planeAtti.w * planeAtti.w + planeAtti.q1 * planeAtti.q1 + planeAtti.q2 * planeAtti.q2 + planeAtti.q3 * planeAtti.q3);
	planeAtti.w *= recipNorm;
	planeAtti.q1 *= recipNorm;
	planeAtti.q2 *= recipNorm;
	planeAtti.q3 *= recipNorm;
    
    //Mahony官方程序到此结束，使用时只需在函数外进行四元数反解欧拉角即可完成全部姿态解算过程
	planeAtti.gx = -gyro->x * 180 / 3.1415926535f;
	planeAtti.gy = -gyro->y * 180 / 3.1415926535f;
	planeAtti.gz = gyro->z * 180 / 3.1415926535f;
	
	planeAtti.az = originA.z;
	accel->x = originA.x;
	accel->y = originA.y;
	accel->z = originA.z;

	gyro->x = originG.x;
	gyro->y = originG.y;
	gyro->z = originG.z;
	QtoRPY();
	return planeAtti;
}

void QtoRPY(void)
{
	float t11, t12, t2, t31, t32;
	t11 = 2 * (planeAtti.w * planeAtti.q1 + planeAtti.q2 * planeAtti.q3);
	t12 = 1 - 2 * (planeAtti.q1 * planeAtti.q1 + planeAtti.q2 * planeAtti.q2);
	planeAtti.pitch = -atan2(t11, t12) * 180.0f / 3.1415926535;

	t2 = 2 * (planeAtti.w * planeAtti.q2 - planeAtti.q3 * planeAtti.q1);
	planeAtti.roll = -asin(t2) * 180.0f / 3.1415926535;

	t31 = 2 * (planeAtti.w * planeAtti.q3 + planeAtti.q1 * planeAtti.q2);
	t32 = 1 - 2 * (planeAtti.q2 * planeAtti.q2 + planeAtti.q3 * planeAtti.q3);
	//planeAtti.yaw = atan2(t31, t32) * 180.0f / 3.1415926535;
}

char Str_Roll[30], Str_Pitch[30], Str_Yaw[30];
void IMU_ShowRPY(Atti atti)
{
	sprintf(Str_Roll, "Roll = %-3.6f    ", atti.roll);
	sprintf(Str_Pitch, "Pitch = %-3.6f    ", atti.pitch);
	sprintf(Str_Yaw, "Yaw = %-3.6f    ", atti.yaw);
	//OLED_Init();
	display_6_8_string(0, 2, (unsigned char *)Str_Roll);
	display_6_8_string(0, 3, (unsigned char *)Str_Pitch);
	display_6_8_string(0, 4, (unsigned char *)Str_Yaw);
}
